#include "servo.h"


#include "pwm.h"
#include "arduino.h"


    char leg_pin[18]={0,  1,  2,
                      3,  4,  5,
                      6,  7,  8,
                      9,  10, 11,
                      12, 13, 14,
                      15, 16, 17};
                      
namespace hexapod { 

    namespace {
        // if freq = 240, 1 tick = 1000000 / (240*4096) ~= 1us
        // if freq = 120, 1 tick = 1000000 / (120*4096) ~= 2us
        const static int kFrequency = 120;
        const static int kTickUs = 2;
        const static int kServoMiddle = 1400;
        const static int kServoMax = 2000;
        const static int kServoMin = 800;
        const static int kServoRange = kServoMax-kServoMiddle;

        auto pwmLeft = hal::PCA9685(0x41);
        auto pwmRight = hal::PCA9685(0x43);
        bool pwmInited = false;
 
        void initPWM(void) {
            if (pwmInited)
                return;

            pwmLeft.begin();  
            pwmLeft.setPWMFreq(kFrequency);
            pwmRight.begin();  
            pwmRight.setPWMFreq(kFrequency);
            pwmInited = true;
        }

        



    }

    void Servo::init(void) {
        initPWM();
    }

    Servo::Servo(int legIndex, int partIndex) {
      //  pwmIndex_ = hexapod2pwm(legIndex, partIndex);
      pwmIndex_=leg_pin[legIndex*3+partIndex];
		//Serial.printf("legindex is :%d partIndex is \r\n",legIndex,partIndex);
        inverse_ = partIndex == 1 ? true : false;
        adjust_angle_ = partIndex == 1 ? 15.0 : 0.0;
        range_ = partIndex == 0 ? 45 : 60;
        angle_ = 0;
    }

    void Servo::setAngle(float angle) {
        initPWM();

        if (angle > range_ + adjust_angle_) {
            //Serial.printf("exceed[%d][%f]\r\n", pwm2hexapod(pwmIndex_), angle);
            angle = range_;
        }
        else if(angle < -range_ + adjust_angle_) {
            //Serial.printf("exceed[%d][%f]\r\n", pwm2hexapod(pwmIndex_), angle);
            angle = -range_;
        }

        angle_ = angle;

        hal::PCA9685* pwm;
        int idx;

        if (pwmIndex_ < 9) {
            pwm = &pwmRight;
            idx = pwmIndex_;

        }
        else {
            pwm = &pwmLeft;
            idx = pwmIndex_ - 9;
        }
     // Serial.println("pwmIndex_");
     // Serial.println(pwmIndex_);
        angle -= adjust_angle_;
        if (inverse_)
            angle = -angle;
angle = -angle;
        int us = kServoMiddle + offset_ + angle*(kServoRange/60)*(1+0.01*scale_);
        if (us > kServoMax)
            us = kServoMax;
        else if(us < kServoMin)
            us = kServoMin;
        // Serial.print("angle:");   
        //Serial.println(angle);
        pwm->setPWM(idx, 0, us/kTickUs);
      // Serial.printf("idx is %d ,setAngle(%.2f, %d)", idx,angle, us);
    }

    float Servo::getAngle(void) {
        return angle_;
    }

}